#include "SniperAssasinateTanksState.h"
#include "World.h"
#include "AttractiveField.h"
#include "PDController.h"
#include "CaptureFlagState.h"
#include "FollowSearchPathState.h"
#include "Graph.h"

bool SniperAssasinateTanksState::targetsEliminated = false;

SniperAssasinateTanksState::SniperAssasinateTanksState(void)
{
}


SniperAssasinateTanksState::~SniperAssasinateTanksState(void)
{
}

Command SniperAssasinateTanksState::GetCommand(MultiAgent* agent) {
	World* world = World::instance();
	tank_t* tank = agent->getTank();
	otank_t* best_enemy = NULL;
	double best_dist = 99999;

	if(tank->status.compare("alive") != 0) return Command(0,0,0,false);

	if (CaptureFlagState::IsFlagCaptured()) {

		Graph g(8);
		g.buildEdges();
		Vector2 start, goal;

		start.x =  tank->pos[0];
		start.y =  tank->pos[1];

		goal.x = world->flags()[3].pos[0];
		goal.y = world->flags()[3].pos[1];
		agent->setState(new FollowSearchPathState(&g, start, goal, false));
		return agent->GetCommand();
	}

	int k = world->other_tanks().size();
	for (int i = 0; i < world->other_tanks().size(); i++) {
		otank_t* otank = &world->other_tanks().at(i);

		if (otank->status.compare("alive") != 0) {
			continue;
		}

		double dist = sqrt(pow(otank->pos[0] - tank->pos[0], 2) + pow(otank->pos[1] - tank->pos[1], 2));
		if (dist < best_dist) {
			best_dist = dist;
			best_enemy = otank;
		}
	}

	if (best_enemy == NULL) {
		SniperAssasinateTanksState::targetsEliminated = true;
		return Command(agent->getTank()->index, 0, 0, false);
	}

	AttractiveField target(Vector2(best_enemy->pos[0], best_enemy->pos[1]));

	Vector2 trajectory = target.get_trajectory(Vector2(tank->pos[0], tank->pos[1]));
	Command cmd = PDController::instance()->GetComand(tank, trajectory);
	cmd.speed(0.0);
	if (abs(cmd.angvel()) < 0.01)
		cmd.shoot(true);

	return cmd;
}

bool SniperAssasinateTanksState::TargetsEliminated() {
	return targetsEliminated;
}